// Selection for how the allocator protects itself from concurrent invocations.
// Set no more than one of the following options to non-zero to select the
// desired mode:
// -> LOCKING_FREERTOS_SEMA to use the FreeRTOS semaphore.
// -> LOCKING_FREERTOS_TASK to use the FreeRTOS task suspend.
// -> LOCKING_POSIX_PTHREAD to use the POSIX pthread mutex.
#define LOCKING_FREERTOS_SEMA 0
#define LOCKING_FREERTOS_TASK 0
#define LOCKING_POSIX_PTHREAD 1

// Sets the minimum allocation size (2^MINIMUM_ORDER) in bytes. This means that
// the allocator will never hand out a chunk of memory less than this size,
// even if requested. You want this value as high as you are willing to
// tolerate for your use case as it will lead to less allocator data overhead
// for the same amount of managed memory.
#define MINIMUM_ORDER 4

// If set to non-zero, this option compiles in sanity-checking code that
// verifies the integrity of the allocator's internal data structures. Yes,
// this makes the resulting binary noticeably bigger, but it does help catch
// memory corruption issues causing odd failures otherwise. If the allocator
// spots anything amiss, it will invoke the AllocatorPanic function with the
// error message.
#define DEBUG 1

#if LOCKING_FREERTOS_SEMA + LOCKING_FREERTOS_TASK + LOCKING_POSIX_PTHREAD > 1
 #error "You can pick at most one LOCKING option."
#endif
#if MINIMUM_ORDER < 3
 #error "MINIMUM_ORDER must be at least 3."
#endif

#if LOCKING_FREERTOS_SEMA
 #include <portmacro.h>
 #include <semphr.h>
#endif
#if LOCKING_FREERTOS_TASK
 #include <task.h>
#endif
#if LOCKING_POSIX_PTHREAD
 #include <pthread.h>
#endif
#include <stdint.h>
#include <string.h>
#include "allocator.h"

#if LOCKING_FREERTOS_SEMA
 static SemaphoreHandle_t alloc_lock;
#endif
#if LOCKING_POSIX_PTHREAD
 static pthread_mutex_t alloc_lock;
#endif

static void initialize_lock(void)
{
#if LOCKING_FREERTOS_SEMA
	alloc_lock = xSemaphoreCreateBinary();
#endif
#if LOCKING_POSIX_PTHREAD
	pthread_mutex_init(&alloc_lock, NULL);
#endif
}

static void acquire_lock(void)
{
#if LOCKING_FREERTOS_SEMA
	xSemaphoreTake(alloc_lock, portMAX_DELAY);
#endif
#if LOCKING_FREERTOS_TASK
	vTaskSuspendAll();
#endif
#if LOCKING_POSIX_PTHREAD
	pthread_mutex_lock(&alloc_lock);
#endif
}

static void release_lock(void)
{
#if LOCKING_FREERTOS_SEMA
	xSemaphoreGive(alloc_lock);
#endif
#if LOCKING_FREERTOS_TASK
	xTaskResumeAll();
#endif
#if LOCKING_POSIX_PTHREAD
	pthread_mutex_unlock(&alloc_lock);
#endif
}

struct chunk_entry {
	struct chunk_entry * next;
#if DEBUG
#define MAGIC_VALUE 0xC0DA
	uint16_t magic;
	uint8_t order;
#endif
};

static uint8_t order_count = 0;
static struct chunk_entry ** free_chains = NULL;
static uint8_t * chunk_metas = NULL;
static const void * avail_start = NULL;
#if DEBUG
static uint8_t init_complete = 0;
static size_t avail_range = 0;
#endif

static void * align_flip(void * addr, size_t size)
{
	return (void *)((unsigned long)addr ^ size);
}

static void * align_dn(void * addr, size_t size)
{
	return (void *)((unsigned long)addr & ~(size - 1));
}

static void * align_up(void * addr, size_t size)
{
	return align_dn(addr + size - 1, size);
}

#if DEBUG
static int not_aligned(void * addr, size_t size)
{
	return ((unsigned long)addr & (size - 1)) != 0;
}
#endif

void vPortInitialize(void * addr, size_t size)
{
	void * fini;
	void * lower;
	void * upper;
	uint8_t i;
	
#if DEBUG
	if (init_complete) {
		AllocatorPanic("already initialized");
		return;
	}
	init_complete = 1;
#endif
	
	initialize_lock();
	
	fini = addr + size;
	addr = align_up(addr, sizeof(void *));
	fini = align_dn(fini, 1 << MINIMUM_ORDER);
	
	lower = addr;
	upper = fini;
	order_count = 0;
	for (;;) {
		lower = align_up(lower, 1 << (order_count + MINIMUM_ORDER));
		upper = align_dn(upper, 1 << (order_count + MINIMUM_ORDER));
		if (lower >= upper)
			break;
		order_count++;
	}
	
	free_chains = addr;
	for (i = 0; i < order_count; i++)
		free_chains[i] = NULL;
	addr += sizeof(*free_chains) * order_count;
	
	size = (fini - addr) / ((1 << (MINIMUM_ORDER + 2)) + 1) + 1;
	chunk_metas = addr;
	memset(chunk_metas, 0, sizeof(*chunk_metas) * size);
	addr += sizeof(*chunk_metas) * size;
	
	addr = align_up(addr, 1 << MINIMUM_ORDER);
	size = fini - addr;
	avail_start = addr;
#if DEBUG
	avail_range = size;
#endif
	
	while (addr < fini) {
		uint8_t order, count;
		struct chunk_entry * entry;
		
		order = -1;
		count = __builtin_ctzl((unsigned long)addr);
		if (order > count)
			order = count;
		count = __builtin_ctzl(size);
		if (order > count)
			order = count;
		
		entry = addr;
		addr += 1 << order;
		size -= 1 << order;
		order -= MINIMUM_ORDER;
		
		entry->next = free_chains[order];
#if DEBUG
		entry->magic = MAGIC_VALUE;
		entry->order = order;
#endif
		free_chains[order] = entry;
	}
}

static uint8_t get_metadata(size_t index)
{
	size_t aoffset, boffset;
	const uint8_t * metas;
	
	aoffset = index >> 2;
	boffset = (index & 3) << 1;
	metas = chunk_metas + aoffset;
#if DEBUG
	if ((const void *)metas >= avail_start) {
		AllocatorPanic("bogus pointer");
		return 0;
	}
#endif
	return (*metas >> boffset) & 3;
}

static void set_metadata(size_t index, uint8_t meta)
{
	size_t aoffset, boffset;
	uint8_t * metas;
	
	aoffset = index >> 2;
	boffset = (index & 3) << 1;
	metas = chunk_metas + aoffset;
#if DEBUG
	if ((void *)metas >= avail_start) {
		AllocatorPanic("bogus pointer");
		return;
	}
#endif
	*metas &= ~(3 << boffset);
	*metas |= meta << boffset;
}

#if DEBUG
static int unmanaged_pointer(const void * addr)
{
	const void * limit;
	
	limit = avail_start;
	if (addr < limit)
		return 1;
	limit += avail_range - 1;
	if (addr > limit)
		return 1;
	return 0;
}
#endif

void * pvPortMalloc(size_t size)
{
	uint8_t order, find;
	void * ptr;
	size_t index;
	
#if DEBUG
	if (!init_complete) {
		AllocatorPanic("not initialized");
		return NULL;
	}
#endif
	
	for (order = 0; (1 << (order + MINIMUM_ORDER)) < size; order++);
	ptr = NULL;
	
	acquire_lock();
	
	for (find = order; find < order_count; find++) {
		struct chunk_entry * entry;
		
		entry = free_chains[find];
		if (entry != NULL) {
#if DEBUG
			if (not_aligned(entry, 1 << (find + MINIMUM_ORDER)) ||
			    get_metadata(((void *)entry - avail_start) >>
			                 MINIMUM_ORDER) != 0 ||
			    entry->magic != MAGIC_VALUE ||
			    entry->order != find) {
				AllocatorPanic("memory corruption");
				goto done;
			}
#endif
			free_chains[find] = entry->next;
			ptr = entry;
			break;
		}
	}
	if (ptr == NULL)
		goto done;
	while (find != order) {
		struct chunk_entry * entry;
		
#if DEBUG
		if (get_metadata((ptr - avail_start) >> MINIMUM_ORDER) != 0) {
			AllocatorPanic("memory corruption");
			goto done;
		}
#endif
		find--;
		entry = ptr;
		entry->next = NULL;
#if DEBUG
		entry->magic = MAGIC_VALUE;
		entry->order = find;
#endif
		free_chains[find] = entry;
		
		ptr += 1 << (find + MINIMUM_ORDER);
	}
	
	index = (ptr - avail_start) >> MINIMUM_ORDER;
	set_metadata(index, 1);
	while (order != 0) {
		index++;
		set_metadata(index, (order & 1) | 2);
		order >>= 1;
	}
	
done:
	release_lock();
	
#if DEBUG
	if (ptr != NULL && unmanaged_pointer(ptr)) {
		AllocatorPanic("bogus pointer");
		return NULL;
	}
#endif
	
	return ptr;
}

void vPortFree(void * ptr)
{
	size_t index, i;
	uint8_t order;
	struct chunk_entry * entry;
	
#if DEBUG
	if (!init_complete) {
		AllocatorPanic("not initialized");
		return;
	}
#endif
	
	if (ptr == NULL)
		return;
#if DEBUG
	if (not_aligned(ptr, 1 << MINIMUM_ORDER) ||
	    unmanaged_pointer(ptr)) {
		AllocatorPanic("bogus pointer");
		return;
	}
#endif
	
	acquire_lock();
	
	index = (ptr - avail_start) >> MINIMUM_ORDER;
#if DEBUG
	if (get_metadata(index) != 1) {
		AllocatorPanic("bogus pointer");
		goto done;
	}
#endif
	set_metadata(index, 0);
	index++;
	order = 0;
	for (i = 0;; i++) {
		uint8_t meta;
		
		meta = get_metadata(index + i);
		if ((meta & 2) == 0) {
#if DEBUG
			if (i != 0 &&
			    (order & (1 << (i - 1))) == 0) {
				AllocatorPanic("memory corruption");
				goto done;
			}
#endif
			break;
		}
#if DEBUG
		if (i == 8) {
			AllocatorPanic("memory corruption");
			goto done;
		}
#endif
		set_metadata(index + i, 0);
		order |= (meta & 1) << i;
	}
#if DEBUG
	if (order >= order_count ||
	    not_aligned(ptr, 1 << (order + MINIMUM_ORDER))) {
		AllocatorPanic("memory corruption");
		goto done;
	}
#endif
	
	for (; order < order_count - 1; order++) {
		const void * buddy;
		struct chunk_entry * prev;
		struct chunk_entry * iter;
		
		buddy = align_flip(ptr, 1 << (order + MINIMUM_ORDER));
		prev = (struct chunk_entry *)(free_chains + order);
		iter = free_chains[order];
		while (iter != NULL) {
#if DEBUG
			if (not_aligned(iter, 1 << (order + MINIMUM_ORDER)) ||
			    get_metadata(((void *)iter - avail_start) >>
			                 MINIMUM_ORDER) != 0 ||
			    iter->magic != MAGIC_VALUE ||
			    iter->order != order) {
				AllocatorPanic("memory corruption");
				goto done;
			}
#endif
			if (buddy == iter) {
				prev->next = iter->next;
				buddy = NULL;
				break;
			}
			prev = iter;
			iter = iter->next;
		}
		if (buddy != NULL)
			break;
		ptr = align_dn(ptr, 1 << (order + MINIMUM_ORDER + 1));
	}
	
	entry = ptr;
	entry->next = free_chains[order];
#if DEBUG
	entry->magic = MAGIC_VALUE;
	entry->order = order;
#endif
	free_chains[order] = entry;
	
#if DEBUG
done:
#endif
	release_lock();
}
